#include "ObstacleAvoidanceActionGroup.h"
ObstacleAvoidanceActionGroup::ObstacleAvoidanceActionGroup(ArRobot* robot,
		int maxVelocity, int rotVel, uint16_t distanceToRealign, double turningTime) :
		ArActionGroup(robot) {
	this->initialDirection = 0;
	this->robot=robot;
	startRealignment = false;
	emergencyStop = false;
	avoid = new AvoidObstacleAction(robot,turningTime,rotVel,&startRealignment);
	advance = new GoStraightAction(robot, maxVelocity, &startRealignment,
			distanceToRealign, &emergencyStop);

	addAction(avoid, 50);
	addAction(advance, 50);
}

ObstacleAvoidanceActionGroup::~ObstacleAvoidanceActionGroup() {
	delete advance;
	delete avoid;
}

void ObstacleAvoidanceActionGroup::start( int direction,
		uint16_t distanceToTravel, double turningTime) {
	initialDirection = direction;
	startRealignment = false;
	avoid->setDeltaHeading(turningTime,direction);
	advance->setDistance(distanceToTravel);
}



bool ObstacleAvoidanceActionGroup::hasCompletedAvoidance() {
	return ((advance->haveAchievedDistance() && avoid->haveAchievedCurve()) || robot->getVel()==0);
}
